00100	IFN USER+TRACK,<0>
00200	STRADR:	MOVE BITSAV	;Enters here every 1/60 th. seccond
00300	IFE USER+TRACK,<
00400		TRNE DTST
00500		CALLI 400024
00600		DATAO ARM,[2]
00700		SOSLE STWT
00800		CALLI 400024
00900	>
01000		MOVE P,[XWD -PDLL,PDL]	;PUSH DOWN STACK
01100		MOVE CBUF,CBUFS
01200		MOVE DATWD,DATWDS
01300		IORI DATWD,2		;YELLOW ARM
01400	IFN REPORT+TRACK,<
01500		MOVE DATA,DATPT
01600	>
01700	IFN USER+TRACK,<
01800		TRNE DTST
01900		JRST SWDONE
02000	>
02100	IFE USER,<
02200	IFN TRACK,<
02300		SKIPE WALK
02400		JRST NCHK
02500	>
02600		CONSZ 40		;CHECK TO SEE IF ON PDP-6
02700		JRST[	MOVEI TAC,PDPERR
02800			MOVEM TAC,TRAJER
02900			JRST TOFF]
03000		CONI 0,AC
03100		ANDI AC,7
03200		CONO 0,2050(AC)
03400		TRZE TRIP		;TURN ON ARM
03500		DATAO ARM,[3]
03600	>
03700	NCHK:
03800		MOVEI K,51B23		;-10 VOLT REFERENCE
03900	IFE USER,<PUSHJ P,PREAD		;REFTR← -10 volt reference
04000		LDB AC,SNUM		;PICK UP MIDDLE SAMPLE
04100		ANDI DACVAL,7777	;LEAVE LAST SAMPLE IN J
04200		ADDI AC,(DACVAL)	;ADD THEM TOGETHER
04300	>
04400	IFN USER,<MOVEI AC,17770>
04500		CAIGE AC,17000		;IS THE FUSE BLOWN
04600		JRST[		MOVEI TAC,REFERR
04700				MOVEM TAC,TRAJER
04800				JRST TOFF]
04900		MOVEM AC,REFTR		;REFERENCE READING
05000	IFN TRACK,<
05100		SKIPE WALK
05200		JRST TOK
05300	>
05310		MOVEI AC,=8888
05328		MOVEM AC,ETIME
05346		MOVSI AC,(0.5)
05364		MOVEM AC,TDF
05400	IFE USER,<
05500		CONI CLOCK,AC
05600		TLZ AC,777774
05700		JRST STIME		;TIME IN MICRO SECONDS
05800	>
05900	IFN USER,<JRST .+2>
     

00100	NXTJIF:
00200	IFN USER,<JRST SWDONE>
00300	IFN DEB<
00400		MOVEM 17,ACSAV+17
00500		MOVEI 17,ACSAV
00600		BLT 17,ACSAV+16
00700		MOVE 17,ACSAV+17
00800	>
00900	IFN TRACK,<
01000		SKIPE WALK
01100		JRST TOK
01200	>
01300	IFN REPORT+TRACK,<
01400		HRRZI TAC,(DATA)
01500		CAIL TAC,DATO+DBL-1
01600		JRST[	SUB DATA,[XWD DBL,DBL]
01700			MOVE TAC,[XWD DATO+DBL,DATO]
01800			BLT TAC,(DATA)
01900			HRRZI AC,(DATA)
02000			HRRZ TAC,DATIWD
02100			SUBI AC,(TAC)
02200			JRST RDP]
02300		HRRZI AC,(DATA)
02400		HRRZ TAC,DATIWD
02500		SUBI AC,(TAC)
02600		HRRZ MQ,DATPT
02700		SUBI TAC,(MQ)
02800		IMUL AC,TAC
02900	RDP:	SKIPLE AC
03000	IFN TRACK,<PUSH DATA,[0]>
03100	IFN REPORT,<PUSH DATA,[SIXBIT/ERROR/]>
03200		MOVEI AC,DATO-1
03300		HRRZ TAC,DATIWD
03400	 	CAIL TAC,DATO+DBL-1
03500		HRRM AC,DATIWD
03600		MOVEM DATA,DATPT
03700	>
03800	IFE USER,<
03900		MOVEI TAC,=2000
     

00100	WATSTL:	CONI CLOCK,AC
00200		TLZ AC,777774
00300		SUB AC,LTIME
00400		JUMPGE AC,.+2
00500		ADD AC,[=1000000]
00600	IFN REPORT,<
00700		CAIN TAC,=2000
00800		JRST[	PUSH DATA,[<SIXBIT/TICK/>+1]
01000			PUSH DATA,AC
01100			JRST .+1]
01200	>
01300		MOVEM AC,ETIME
01400		TRNE RUN
01500		JRST[	MOVE MQ,AC
01600			FSC MQ,233
01700			FMPR MQ,[0.00006]
01800			MOVEM MQ,TDF
01900			MOVE AC,LTIME
02000			ADD AC,ETIME
02100			JRST SSTIME]
02200		CAIGE AC,=16667		;delta T greater than 16.7 msec.?
02300		SOJGE TAC,WATSTL
02400		JUMPL TAC,TE
02900		MOVSI AC,(1.0)
03000		MOVEM AC,TDF
03100		MOVE AC,LTIME
03200		ADDI AC,=16667
03300	SSTIME:	TLNE AC,777774
03400		SUB AC,[=1000000]
03500	STIME:	MOVEM AC,LTIME		;LTIME←LTIME+16.7 msec.
03600	>
04300		SOSGE COUNT
04400		JRST[TE:MOVEI AC,TERR
04500			MOVEM AC,TRAJER
04600			JRST ZITO]
04700	IFN ADJUST,<
04800		MOVEI K,14B23
04900		MOVEM BITSAV
05000		MOVEI I,2
05100	RNP:	PUSHJ P,PREAD
05200		LDB AC,SNUM
05300		FSC AC,221
05400		JSA 16,EXP
05500		JUMP AC
05600		FMPR [0.15]
05700		MOVEM FACTOR(I)
05800		SOJGE I,RNP
05900		MOVE BITSAV
06000		MOVE I,ADJI
06100		JUMPL I,MAIN
06200	IFN ADJG,<
06300		MOVE AC,ADJE
06400		FMPR AC,FACTOR
06500		MOVEM AC,KE(I)		;POT 12
06600		MOVE AC,ADJV
06700		FMPR AC,FACTOR+1
06800		MOVEM AC,KV(I)		;POT 13
06900		MOVE AC,ADJIF
07000		FMPR AC,FACTOR+2
07100		MOVEM AC,KI(I)	;POT 14
07200	>
07300	IFN ADJD,<
07400		MOVE AC,ADJE
07500		FMPR AC,FACTOR
07600		MOVEM AC,KM(I)		;POT 12
07700		MOVE AC,ADJV
07800		FMPR AC,FACTOR+1
07900		MOVEM AC,PK(I)		;POT 13
08000	>
08100	>
     

00100	MAIN:	TRNE RUN
00200		JRST[	PUSHJ P,EVAL
00300			PUSHJ P,NXTH
00400			JRST .+1]
00500		MOVEI J,6
00600		PUSHJ P,ANGLES		;read joint angles
00700		TRNE RONLY		;read angles only?
00800		JRST[	PUSHJ P,HEAD	;read hand also
00900			MOVE AC,HAND
01000			MOVEM AC,SET
01100			PUSHJ P,REFLEX
01200			PUSHJ P,READED]	;DOES NOT RETURN
01300		TRNN RUN
01400		JRST[	SKIPG TAC,FUNCT	;not run, mabye a function
01500			PUSHJ P,SETSET
01600			JUMPL TAC,NEXT
01700			JUMPE TAC,[CAIL CBUF,BUF;try next buffer
01800				SETOM (CBUF)	;dont do this if first buffer
01900				MOVEM CBUF,CBUFS
02000				ADDI CBUF,1001
02100				CAILE CBUF,BUF2
02200				MOVEI CBUF,BUF		;Advance buffer
02300				MOVE 2,(CBUF)
02400				JUMPL 2,[BUFWT:MOVE CBUF,CBUFS;wait for buffer
02500					JRST SWDONE]
02600				JUMPN 2,BUFRDY		;the last buffer stop.
02700				JRST[	TRO DTST
02800					MOVEI 2,WAITIN
02900					HRRM 2,JUMPBK
03000					MOVEM 2,STKPTR
03100			IFN REPORT,<PUSH DATA,[0]>
03200					JRST BUFWT]
03300			BUFRDY:	HRRZ 2,(CBUF)
03400				ADDI 2,-1(CBUF)
03500				HRRZM 2,STKPTR
03600				JRST NEXT]
03700			CAIN TAC,6
03800			JRST SETCH
03900			JRST TT]	;otherwise perform function
04000		JUMPE J,[RUDONE:
04100			TRNN NSET
04200			PUSHJ P,SETSET
04300			TRZ DROP+RUN+FINAL+WOB	 	;if J is zero then end of run
04400			TDZ DATWD,[770001252500]
04500			TRZE STP
04600			JRST[	MOVEI TAC,STPERR
04700				MOVEM TAC,TRAJER
04800				POP P,TAC
04900				JRST TOFF]
05000			SKIPN AC,FUNCT
05100			JRST NEXT
05150			CAIN AC,22
05175			JRST[	TRZ HCL
05184				SETZM TDAT
05188				SETZM FUNCT
05190				DATAO DEVT,TDAT
05193				JRST NEXT]
05200			MOVEI AC,=16667
05300			MOVEM AC,ETIME
05400			JRST .+1]
     

00100	TT:	PUSHJ P,REFLEX
00200	DISP:	MOVE AC,FUNCT
00250		JUMPL AC,.+2
00300		CAILE AC,MAXFN
00400		JRST[	MOVEI TAC,FUNERR
00410			MOVEM TAC,TRAJER
00420			JRST TOFF]
00500		JRST @.+1(AC)		;Perform function
00600		NXTJIF	;0
00700		HOPEN	;1
00800		HCLOSE	;2
00900		NXTJIF	;3
01000		PLACE	;4
01100		NUDGE	;5
01200		SETCH	;6
01300		STOP	;7
01400		SAVE	;10
01500		RESTORE	;11
01600		CENTER	;12
01700		SET.ARM	;13 SET_ARM
01800		WOBBLE	;14
01900		SEARCH	;15
02000		AOJI	;16
02100		SLAVE	;17
02200		HOME	;20
02300		PATH	;21
02400		TOOL	;22
02500	
02600		XLIST
02700		SUBTTL	SPACEWAR CODE	ANGLE MEASUREMENT	LOU PAUL
02800		LIST
     

00100	ANGLES:	MOVEI I,5
00200		JFCL 10,.+1
00300		MOVE TAC,[XWD TH,THP]
00400		BLT TAC,THP+5
00500		MOVEI L,10
00600	IFN REPORT,<PUSH DATA,[<SIXBIT/THETA/>+6]>
00700	SETCHA:	MOVEI K,CHA
00800		TRNE ALT6
00900		MOVEI K,N6CHA
01000		SKIPA
01100	RSTRT:	MOVE K,CHAN(I)
01200		SOJL L,REDER
01300	IFE USER,<
01400		PUSHJ P,PREAD
01500		LDB AC,SNUM
01600		CAIN I,5
01700		JRST[	TRNE POTREAD
01800			JRST .+1
01900			CAIL AC,1000
02000			CAILE AC,7000
02100			JRST[	TRC ALT6
02200				JRST SETCHA]
02300			MOVEI K,CHA5
02400			JRST .+1]
02500		ANDI DACVAL,7777
02600		ADDI AC,(DACVAL)
02700		SETZ TAC,
02800		DIV AC,REFTR
02900		ASH AC,-26
03000		MOVE TAC1,AC
03100		TRNE POTREAD
03200		JRST[	ADDI TAC1,=1000
03300			JRST EXON]
03400		ASHC AC,-11		;LEAVE INDEX INTO NON-LINEAR DIFF TABLE IN AC
03500		LDB MQ,NON(I)		;GET LOWER DIFF
03600		ADDI TAC1,(MQ)		;ADD IT TO READING
03700		AOJ AC,
03800		LDB AC,NON(I)		;GET UPPER DIFFERENCE
03900		SUBI AC,(MQ)
04000		MUL TAC,AC		;INTERPOLATE
04100		ADD TAC1,TAC		;CORRECTED READING
04200		FSC TAC1,226		;FLOAT
04300		FMPR TAC1,SCALE(I)
04400		FADR TAC1,OFF(I)
04500	>
04600	IFN USER,<
04700		MOVE TAC1,ERR(I)
04800		FSC TAC1,-1
04900		FADR TAC1,T0(I)
05000	>
05100		CAIN I,5
05200		JRST[	TRNE ALT6
05300			FADR TAC1,OFF6
05400			FADR TAC1,ROT6
05500			MOVEM TAC1,TH+5
05600			FSBR TAC1,T0+5
05700			MOVSI MQ,(180.0)
05800			CAML TAC1,MQ
05900			JRST[	INCROT:	MOVN MQ,MQ
06000					FSC MQ,1
06100					FADRM MQ,ROT6
06200					FADRM MQ,TH+5
06300					JRST THS]
06400			MOVN MQ,MQ
06500			CAMG TAC1,MQ
06600			JRST INCROT
06700			JRST THS]
06800	EXON:	MOVEM TAC1,TH(I)		;THETA
06900	THS:	CAILE I,2
07000		JRST[	MOVE AC,TH(I)
07100			FSBR AC,THP(I)
07200			FDVR AC,TDF
07300			MOVEM AC,TD(I)
07400			JRST SERVO]
07500	IFN REPORT,<
07600		MOVE MQ,TH(I)
07700		FSBR MQ,T0(I)
07800		PUSH DATA,MQ
07900	>
     

00100	IFE USER,<
00200		MOVE K,VCHAN(I)
00300	NVEL:	PUSHJ P,PREAD
00400		LDB AC,SNUM
00500		ANDI DACVAL,7777
00600		ADDI AC,(DACVAL)
00700		TRNE VZERO
00800		MOVEM AC,VZ(I)
00900		SUB AC,VZ(I)
01000		FSC AC,216
01100		FMPR AC,VSCALE(I)
01200	IFN TACH,<
01300		TRNE VZERO
01400		SETZM VDTH(I)
01500		FADRM AC,VDTH(I)
01600	>
01700		MOVEM AC,TD(I)
01800	>
01900		XLIST
02000				SUBTTL			SPACEWAR CODE			SERVO CALC.			LOU PAUL
02100		LIST
     

00100	SERVO:	TRNN RUN
00200		JRST[	SOJGE I,RSTRT
00300			POPJ P,]
00400	SERVL:	MOVE TAC,TH(I)
00500		FSBR TAC,T0(I)		;POSITION ERROR
00600		MOVEM TAC,ET0(I)
00700		TRNE FINAL
00800		JRST NULL		;MODIFY THETA BY INTEGRAL TERM
00900		MOVE TAC1,T0(I)
01000		FSBR TAC1,T0P(I)
01100		FDVR TAC1,TDF
01200		MOVE AC,TAC1
01300		FSBR AC,TDP(I)
01400		MOVEM TAC1,TDP(I)
01500		FDVR AC,TDF
01600		MOVEM AC,TDD(I)
01700		FMPR AC,CII(I)
01950	MODT0:	HRRZ K,BMASK(I)
02000		TDNN K,FBI
02100		JRST [	PDIR:SETZM ET0(I)
02150			FMPR AC,CII(I)
02200			JRST NOBACK]
02300		FADRM TAC,ERRINT(I)
02700	IFE STEP,<
02800		MOVN TAC,TD(I)
02900		FADR TAC,TAC1
03000	>
03100	IFN STEP,<MOVN TAC,TD(I)>
03200		FMPR TAC,KV(I)		;VELOCITY GAIN
03300		FADR AC,TAC
03350		FMPR AC,CII(I)
03400		MOVN TAC,ET0(I)
03500		FMPR TAC,KE(I)		;ERROR GAIN
03700		FADR AC,TAC
03710		MOVN TAC,ERRINT(I)
03720		FMPR TAC,KI(I)
03820		FDVR TAC,CII(I)
03900		FADR AC,TAC
04000	NOBACK:	CAIE I,2
04100		FMPR AC,[IRAD:0.0174532925]	;JOINT 3 IS IN INCHES
04200		MOVEM AC,TORE(I)
04300		FADR AC,CI(I)		;GRAVITY TERM
04400		MOVEM AC,T(I)			;ERROR CORRECTION TORQUE
04500		XLIST
04600				SUBTTL			SPACEWAR CODE			DRIVE CALC.			LOU PAUL
04700		LIST
     

00100	DRIVE:
00200	IFN REPORT,<PUSH DATA,[<SIXBIT/DAC/>+6]>
00300	LS0:	TDNN DATWD,BMASK(I)
00400		JRST ISP
00500		MOVE MQ,TD(I)
00600		JUMPN MQ,.+2
00700		MOVN MQ,ET0(I)
00800		FMPR MQ,T(I)
00900		JUMPL MQ,[	MOVM MQ,T(I)
01000				FSBR MQ,F0(I)
01100				JUMPL MQ,[	FDVR MQ,F0(I)
01200						FMPR MQ,V0(I)
01300						FSC MQ,1
01400						FADR MQ,V0(I)
01500						SKIPGE T(I)
01600						MOVN MQ,MQ
01700						JRST CEMF]
01800				MOVSI AC,(1.0)
01900				FSBR AC,PK(I)
02000				FMPR MQ,AC
02100				FMPR MQ,KM(I)
02200				FADR MQ,V0(I)
02300				SKIPG T(I)
02400				MOVN MQ,MQ
02500				JRST CEMF]
02600		SKIPN T(I)
02700		JRST[		SKIPN TD(I)
02800				JRST CEMF
02900				MOVE MQ,V0(I)
03000				SKIPGE TD(I)
03100				MOVN MQ,MQ
03200				JRST CEMF]
03300		JUMPE MQ,NOEX
03400		MOVE MQ,V0(I)
03500		SKIPGE T(I)
03600		MOVN MQ,MQ
03700	NOEX:	MOVSI AC,(1.0)
03800		FADR AC,PK(I)
03900		FMPR AC,T(I)
04000		FMPR AC,KM(I)
04100		FADR MQ,AC
04200	CEMF:
04300		MOVE AC,EMF(I)
04400		FMPR AC,TD(I)
04500		MOVSI TAC1,(30.0)
04600		JUMPGE MQ,DRVLT
04700		MOVN AC,AC
04800	DRVLT:	FSBR TAC1,AC			;Available drive voltage
04900		CAMGE TAC1,[1.0]
05000		JRST MAXDRV
05100	VELOK:	MOVM AC,MQ
05200		FDVR AC,TAC1		;AC= REL TIME ON
05300		FIX AC,211000		;1=16
05400		CAML AC,[4000000]
05500		JRST DHO		;too much force stop the arm
05600		CAILE AC,776000
05700	MAXDRV:	MOVEI AC,776000
05800	IFN REPORT+TRACK+USER+DEB,<
05900		MOVEM AC,MOTOR(I)
06000		SKIPGE MQ
06100		MOVNS MOTOR(I)
06200	>
06300		TRC AC,400000
06400		HRL AC,I
06500	SETDRV:
06600	IFN TRACK,<SKIPN WALK>
06700		DATAO WIDTH,AC
06800		TDZ DATWD,DMASK(I)
06900		JUMPGE MQ,ISP
07000		TDO DATWD,DMASK(I)
07100	ISP:	SKIPE REV(I)
07200		TDC DATWD,DMASK(I)
07300	IFN REPORT,<PUSH DATA,MOTOR(I)>
07400		DATAO ARM,DATWD
07500		SOJGE I,RSTRT
07600		JFCL 10,DHO
07700	IFN TRACK,<SKIPN WALK>
     

00100		TRZ VZERO
00200		SETZM ASTP
00300		TRNE STP
00400		JRST[	MOVEI I,5
00500		MTL:	MOVE MQ,TORE(I)
00600			FDVR MQ,F0(I)
00700			SKIPN TD(I)
00800			FSC MQ,-2
00900			FMPR MQ,STQ(I)
01000			FADRM MQ,ASTP
01100			SOJGE I,MTL
01200			MOVE AC,ASTP
01300			CAML AC,SDTQ
01400			JRST[	AOS AC,ASTPP
01500				CAIL AC,2
01600				JRST[	TRZ STP
01700					SUB P,[XWD 1,1]
01800					SETZM ERRINT
01900					MOVE AC,[XWD ERRINT, ERRINT+1]
02000					BLT AC,ERRINT+5
02100					JRST RUDONE]
02200				POPJ P,]
02300			SETZM ASTPP
02400			POPJ P,]
02500		POPJ P,
02600	
02700	NULL:	SETZB AC,TAC1
02800		TLNN DATWD,NNUL
02900		TRNE WOB
03000		SOJA J,MODT0
03100		TDNN DATWD,GMASK(I)
03200		SOJA J,DRIVE
03300		MOVE MQ,FBI
03400		TDNN MQ,GMASK(I)
03500		SOJA J,PDIR
03600		MOVM MQ,TAC			;|POSITION ERROR|
03700		CAMLE MQ,ERR(I)			;COMPARE POSITION ERROR
03800		JRST MODT0
03900		MOVM MQ,TD(I)
04000		CAMLE MQ,ERR(I)
04100		JRST MODT0
04200	SO:	TDZ DATWD,BMASK(I)
04300	IFN REPORT+TRACK+USER+DEB,<SETZM MOTOR(I)>
04400		DATAO ARM,DATWD
04500		HRLZ MQ,I
04600		TRC MQ,400000
04700		DATAO WIDTH,MQ
04800		SOJA J,DRIVE
04900		POPJ P,
05000	
05100		XLIST
05200		SUBTTL	SPACEWAR CODE				LOU PAUL
05300		LIST
     

00100	NXTH:	MOVE AC,[XWD T0,T0P]
00200		BLT AC,T0P+5
00300		TRNE WOB
00400		JRST[	MOVEI I,2
00500		WL1:	SOSGE J,WOBCNT(I)
00600			JRST[	MOVEI J,=19
00700				MOVEM J,WOBCNT(I)
00800				JRST WL2]
00900		WL2:	MOVE AC,SIN(J)
01000			FMPR AC,WOBMAG
01100			FADRM AC,TFF+3(I)
01200			SOJGE I,WL1
01300			JRST .+1]
01400		TRNE DROP
01500		JRST[	MOVEI I,5
01600		OL:	MOVE AC,DELTH(I)
01700			FMPR AC,TDF
01800			FADRM AC,DTH(I)
01900			SOJGE I,OL
02000			JRST JALS]
02100	NTH:
02200		TRNE INCREM
02300		JRST[	MOVE MQ,ETIME
02400			ADDB MQ,TICKS
02500			CAML MQ,NTICKS
02600			JRST[	TRZ INCREM
02700				SKIPN TP
02800				TRO FINAL
02900				MOVEI AC,=600
03000				MOVEM AC,COUNT
03005				SKIPN TP(I)
03010				TLNN DATWD,INT
03020				JRST FIL1
03100				MOVE AC,[XWD NC,CI]
03200				BLT AC,CII+5
03300			FIL1:	MOVEI I,5
03400			FIL:	MOVE AC,DELTH(I)
03500				FADRB AC,DTH(I)
03600				FADR AC,TFF(I)
03700				MOVEM AC,T0(I)
03800				SETZM DELTH(I)
04100				SOJGE I,FIL
04200				POPJ P,]
04300			SETZ I,
04400			DIV MQ,NTICKS
04500			ASH MQ,-10
04600			FSC MQ,200
04700			MOVSI AC,(6.0)
04800			FMPR AC,MQ
04900			FADR AC,[-15.0]
05000			FMPR AC,MQ
05100			FADR AC,[10.0]
05200			FMPR AC,MQ
05300			FMPR MQ,MQ
05400			FMPR MQ,AC
05500			MOVEI I,5
05600		LDC:	MOVE AC,DELTH(I)
05700			FMPR AC,MQ
05800			FADR AC,DTH(I)
05900			FADR AC,TFF(I)
06000			MOVEM AC,T0(I)
06005			SKIPN TP
06010			TLNN DATWD,INT
06020			JRST LDC1
06100			MOVE AC,DCI(I)
06200			FMPR AC,MQ
06300			FADR AC,PC(I)
06500			MOVEM AC,CI(I)
06600			MOVE AC,DCII(I)
06700			FMPR AC,MQ
06800			FADR AC,PCC(I)
06900			MOVEM AC,CII(I)
07000		LDC1:	SOJGE I,LDC
07100			POPJ P,]
07200	JALS:	MOVEI I,5
07300	JAL:	MOVE AC,DTH(I)
07400		FADR AC,TFF(I)
07500		MOVEM AC,T0(I)
07600		SOJGE I,JAL
07700		POPJ P,
07800	
     

00100	EVAL:	MOVEI I,5
00200	EL:	MOVE J,TP(I)	;POINTS TO KOE5
00300		JUMPE J,NE
00400		MOVE MQ,ETIME
00500		ADDB MQ,TJ(I)
00600		CAML MQ,TN(I)	;NUMBER OF TICKS THIS SEGMENT
00700		JRST[			;END OF SEGMENT
00800			CAIN I,5
00900			JRST[			;JOINT 6 HAS CONTROL WORD
01000				MOVE TAC,TICKS
01100				CAMGE TAC,NTICKS
01200				TRO INCREM
01300				LDB TAC,[POINT 3,1(J),2]	;LEVEL
01400				JUMPN TAC,[			;SWITCH POINT
01500					SKIPG LOOP(TAC)
01600					JRST[			;INITIALIZE LOOP
01700						LDB AC,[POINT 15,1(J),17]
01800						MOVEM AC,LOOP(TAC)	;COUNT
01900						JRST SETALT]
02000					SOSLE LOOP(TAC)		;IN LOOP
02100				SETALT:	TRO ALT
02200					JRST GETNXT]
02300				JRST GETNXT]
02400		GETNXT:	TRNE ALT
02500			JRST[	LDB J,[POINT 9,(J),8]	;ALT POINTER
02600				JRST GOT]
02700			LDB J,[POINT 9,(J),17]		;NEXT POINTER
02800			JUMPE J,[SETZM TP(I)
02900				TRO FINAL
03000				MOVEI AC,=600
03100				MOVEM AC,COUNT
03200				MOVE TAC,[XWD NC,CI]
03300				BLT TAC,CI+=11
03400				MOVE AC,A4(I)
03500				ADD AC,A3(I)
03600				ADD AC,A2(I)
03700				ADD AC,A1(I)
03800				ADD AC,A0(I)
03900				XOR AC,SC(I)
04000				FSC AC,0
04100				MOVEM AC,TFF(I)
04200				JRST NE]
04300		GOT:	ADDI J,(CBUF)
04400			MOVEM J,TP(I)
04500			SUB MQ,TN(I)	;MICRO SEC INTO NEW SEGMENT
04600			MOVEM MQ,TJ(I)	;INITIALIZE
04700			HRRZ AC,(J)
04800			ASH AC,16
04900			MOVEM AC,TN(I)
05000			CAIN I,5
05100			JRST[
05200				HRRZ AC,1(J)
05300				ADDI AC,(CBUF)
05400				MOVEI MQ,6(AC)
05500				MOVEI K,5
05600				MOVE MQ,(MQ)		;NEW DATWD
05700			FTF:	MOVE TAC,GMASK(K)
05800				TDNN TAC,FBI
05900				JRST[	TDNN TAC,MQ	;WAS FREE
06000					JRST FNF	;STILL FREE
06100					MOVE TAC,TH(K)
06200					MOVEM TAC,T0(K)
06300					FSBR TAC,TFF(K)
06400					MOVEM TAC,DTH(K)
06500					MOVNM TAC,DELTH(K)
06600					MOVE TAC,TN+5
06700					ASH TAC,16
06800					MOVEM TAC,NTICKS
06900					SETZM TICKS
07000					TRO INCREM
07100					JRST FNF]
07200				TDNN TAC,MQ		;NOT FREE
07300				JRST[	MOVE TAC,TDD(K)	;NOW IS FREE
07400					CAIE K,2
07500					FMPR TAC,IRAD
07600					FMPR TAC,CII(K)
07650					FMPR TAC,CII(K)
07700					FSBR TAC,T(K)
07800					MOVNM TAC,NC(K)
07900					JRST FNF]
08000			FNF:	SOJGE K,FTF
08010				PUSHJ P,UPDC
08100				JRST DDELC]
08200		DDELC:	HLRE AC,-3(J)
08300			MOVEM AC,A4(I)
08400			HRRE AC,-3(J)
08500			MOVEM AC,A3(I)
08600			HLRE AC,-2(J)
08700			MOVEM AC,A2(I)
08800			HRRE AC,-2(J)
08900			MOVEM AC,A1(I)
09000			HLLZ AC,-1(J)
09100			MOVEM AC,SC(I)
09200			HRRE AC,-1(J)
09300			MOVEM AC,A0(I)
09400			MOVE MQ,TJ(I)
09500			JRST ELL]
09600	ELL:	DIV MQ,TN(I)
09610		TLNE DATWD,INT
09700		CAIE I,5
09710		JRST ELL1
09800		MOVEI K,=11
09900		HLRE TAC,MQ
10000		FSC TAC,211
10100	DECC:	MOVE AC,DCI(K)
10200		FMPR AC,TAC
10300		FADR AC,PC(K)
10400		MOVEM AC,CI(K)
10500		SOJGE K,DECC
10700	ELL1:	MOVE AC,A4(I)
10800		MUL AC,MQ
10900		ADD AC,A3(I)
11000		MUL AC,MQ
11100		ADD AC,A2(I)
11200		MUL AC,MQ
11300		ADD AC,A1(I)
11400		MUL AC,MQ
11500		ADD AC,A0(I)
11600		XOR AC,SC(I)
11700		FSC AC,0
11800		MOVEM AC,TFF(I)
11900		CAIN I,5
12000		JRST[	MOVE AC,TFF+5
12100			FSBR AC,T0+5
12200			MOVSI MQ,(180.0)
12300			CAML AC,MQ
12400			JRST[	INCT0:	FSC MQ,1
12500					FADRM MQ,T0+5
12600					JRST .+1]
12700			MOVN MQ,MQ
12800			CAMG AC,MQ
12900			JRST INCT0
13000			JRST .+1]
13100	NE:	SOJGE I,EL
13200		TRZ ALT
13300		POPJ P,
13400	
     

00100	DHO:	AOJ I,			;DISASTER HAS OCCURED
00200		LSH I,3			;JOINT NUMBER
00300		IORI I,EXER
00400		MOVEM I,TRAJER
00500		POP P,I
00600	ZITO:	SETZM ERRINT
00700		MOVE AC,[XWD ERRINT,ERRINT+1]
00800		BLT AC,ERRINT+5
00900	
01000	TOFF:	MOVE AC,STKPTR
01100		MOVEM AC,ERRSTK
01150		DATAO DEVT,[0]
01200		MOVEI L,1000
01300		TRNE RONLY
01400		JRST READED
01500		TRZ RUN+FINAL+DROP+INCREM+HCL+STP+WOB		;STOP THE ARM
01600		SETZM TP
01700		MOVE AC,[XWD TP,TP+1]
01800		BLT AC,TP+5
01900		PUSHJ P,SETSET
02000	MOVNXT:	AOS AC,STKPTR
02100	MOVNX:	SOJL L,[ LE:
02200			MOVEI TAC,LOOPER
02300			MOVEM TAC,TRAJER
02400			JRST PW]
02500		HLRE TAC,@STKPTR
02600		CAIN TAC,3		;IS IT A WAIT
02700		JRST MOVNXT
02800		CAIN TAC,SKPERR
02900		JRST[	HRRZ TAC,(AC)
03000			CAMN TAC,TRAJER
03100			JRST[	AOS STKPTR
03200				JRST NEXT]
03300			JRST MOVNXT]
03400		CAIN TAC,SKPNER
03500		JRST[	HRRZ TAC,(AC)
03600			CAME TAC,TRAJER
03700			JRST[	AOS STKPTR
03800				JRST MOVNXT]
03900			JRST NEXT]
04000		CAIN TAC,SKPSER
04100		JRST[	LDB MQ,[POINT 3,(AC),35]
04200			LDB TAC,[POINT 3,TRAJER,35]
04300			CAIN TAC,(MQ)
04400			JRST[	HRRZ TAC,(AC)
04500				ANDI TAC,777770
04600				TDNE TAC,TRAJER
04700				JRST[	AOS STKPTR
04800					JRST NEXT]
04900				JRST MOVNXT]
05000			JRST MOVNXT]
05100		CAIN TAC,JMPREL
05200		JRST[	HRRE TAC,(AC)
05300			ADDI AC,(TAC)
05400			HRRM AC,STKPTR
05500			JRST MOVNX]
05600	PW:	CAIL AC,BUF
05700		HRRM AC,JUMPBK
05800		MOVEI AC,WAITIN
05900		MOVEM AC,STKPTR
06000		MOVEI AC,3
06100		MOVEM AC,FUNCT
06200	IFN TRACK,<SKIPN WALK>
06300		DATAO ARM,[2]		;STOP THE ARM
06400	IFN REPORT,<PUSH DATA,[0]>
06500	READED:	TRO DTST
06600	IFN DEB,<
06700		MOVEM 17,ACSAV+17
06800		MOVEI 17,ACSAV
06900		BLT 17,ACSAV+16
07000	>
07100		JRST SWDONE
07200	
     

00100	NEXT:	TRNE RUN
00200		JRST[	SETZM FUNCT
00300			JRST NXTJIF]
00400		MOVEI L,1000
00500		MOVE AC,FUNCT
00600		CAIE AC,6		;SET TOUCH
00700		JRST[	MOVE TAC,SOTCHS
00800			MOVEM TAC,SOTCH
00900			TRZ STCH
01000			JUMPE TAC,.+1
01100			TRO STCH
01200			JRST .+1]
01300	IFN TRACK,<SKIPN WALK>
01400		MOVEI DATWD,2
01500		DATAO ARM,DATWD		;STOP THE ARM
01600	INCPIC:	AOS AC,STKPTR
01700	PICKUP:	SOJL L,LE
01800		HLRE TAC,@STKPTR
01900		CAIN TAC,JMPABS
02000		JRST[JMPA:HRRZ TAC,@STKPTR
02100			MOVEM TAC,STKPTR
02200			HLRZ TAC,@STKPTR
02300			CAIN TAC,3
02400			JRST INCPIC
02500			JRST PICKUP]
02600		CAIN TAC,SKPERR
02700		JRST[	HRRZ TAC,@STKPTR
02800			CAMN TAC,TRAJER
02900			AOS STKPTR
03000			JRST INCPIC]
03100		CAIN TAC,SKPNER
03200		JRST[	HRRZ TAC,@STKPTR
03300			CAME TAC,TRAJER
03400			AOS STKPTR
03500			JRST INCPIC]
03600		CAIN TAC,SKPSER
03700		JRST[	LDB MQ,[POINT 3,@STKPTR,35]
03800			LDB TAC,[POINT 3,TRAJER,35]
03900			CAIN TAC,(MQ)
04000			JRST[	HRRZ TAC,@STKPTR
04100				ANDI TAC,777770
04200				TDNE TAC,TRAJER
04300				AOS STKPTR
04400				JRST INCPIC]
04500			JRST INCPIC]
04600		CAIN TAC,JMPREL
04700		JRST[	HRRE TAC,@STKPTR
04800			ADD TAC,STKPTR
04900			HRRM TAC,STKPTR
05000			JRST PICKUP]
05100		SETZM TRAJER
05200		MOVEI I,100
05300		MOVEM I,COUNT
05400		TRZE TAC,MERGE
05500		TRO BOTH
05600		MOVEM TAC,FUNCT
05700		CAIN TAC,3		;IS IT A WAIT
05800		JRST[	MOVE TAC,STKPTR
05900			CAIL TAC,BUF
06000			HRRM TAC,JUMPBK
06100			MOVEI TAC,WAITIN
06200			MOVEM TAC,STKPTR
06300	IFN REPORT,<PUSH DATA,[0]>
06400			TRO DTST
06500			PUSHJ P,SWDONE]
06600	MARK:
06700	IFN REPORT,<
06800		PUSH DATA,[<SIXBIT/NEXT/>+1]
06900		MOVE TAC,FUNCT
07000		HRL TAC,0
07100		PUSH DATA,TAC
07200	>
07300	
07400	;	JRST DISP
07500	SWDONE:
07600		MOVEM BITSAV
07700		MOVEM DATWD,DATWDS
07800		MOVEM CBUF,CBUFS
07900	IFN REPORT+TRACK,<
08000		MOVEM DATA,DATPT
08100	>
08200	IFE USER,<
08300	IFN TRACK,<
08400		SKIPE WALK
08500		JRST @STRADR-1
08600	>
08800		CALLI 400024
08900	>
09000	IFN USER,<
09100		JRST @STRADR-1
09200	>
09300	
09400	SETSET:	MOVEI I,5
09500	TCHK:	MOVE J,TP(I)
09600		JUMPE J,ATEND
09700		SETZM TJ(I)
09800		SETZM TN(I)
09900	GNN:	LDB J,[POINT 9,(J),17]
10000		ADDI J,(CBUF)
10100		CAIN J,(CBUF)
10200		JRST[	MOVEM J,TP(I)
10300			JRST ATEND]
10400		JRST GNN
10500	ATEND:	SOJGE I,TCHK
10600		PUSHJ P,EVAL
10700		MOVEI I,5
10800	FDTH:	MOVE AC,TH(I)
10900		MOVEM AC,T0(I)
11000		FSBR AC,TFF(I)
11100		MOVEM AC,DTH(I)
11200		SETZM DELTH(I)
11300		SETZM TDP(I)
11400		SETZM TDD(I)
11500		SOJGE I,FDTH
11600		POPJ P,
11700	
11800	UPDC:	MOVE TAC,[XWD NC,PC]
11900		BLT TAC,PCC+5
12000		HRL AC,AC
12100		HRRI AC,NC
12200		BLT AC,NC+6
12500		MOVE DATWD,NC+6
12600		HRRZM DATWD,FBI
12700		MOVEI K,5
12800	UPL:	MOVE AC,NC(K)
12900		HLLZM AC,NC(K)
13000		HRLZM AC,NCC(K)
13400		MOVE AC,MOTARM(K)
13500		FADRB AC,NCC(K)
13510		TLNN DATWD,INT
13520		JRST UPL1
13600		FSBR AC,PCC(K)
13700		MOVEM AC,DCII(K)
13710		MOVE AC,NC(K)
13720		FSBR AC,PC(K)
13730		MOVEM AC,DCI(K)
13800	UPL1:	TDNE DATWD,BMASK(K)
13900		TDO DATWD,BMASK(K)
14000		SOJGE K,UPL
14010		TLNE DATWD,INT
14100		POPJ P,
14130		MOVE AC,[XWD NC,CI]
14140		BLT AC,CII+5
14150		POPJ P,
     

00100	PREAD:
00200	IFN TRACK,<
00300		SKIPE WALK
00400		JRST[GNPR:AOBJP DATA,[	MOVE AC,[IOWD DBL,DATO]
00500					MOVEM AC,DATPT
00600					MOVE DATA,DATPT
00700					SETZ TAC,
00800					INPUT 15,1
00900					JRST GNPR]
01000			MOVE DACVAL,(DATA)
01100			JRST @READFN]
01200	>
01300		MOVEI TAC,10
01400		MOVEM TAC,DATMSD
01500	DACRED:	SOSGE DATMSD
01600		JRST[REDER:	MOVEI TAC,READER	
01700				MOVEM TAC,TRAJER
01800				JRST TOFF]
01900	DACST:	CONO DB,4250
02000		CONO AD,(K)
02100	NREAD:	MOVEI TAC,30
02200		CONSO DB,1000
02300		SOJGE TAC,.-1
02400		JUMPL TAC,[DACDIE:CONO AD,4000(K)	;STOP DAC
02500				TRO REREAD
02600			IFN DEB,<MOVEM DACVAL,DACERR>
02700				MOVEI TAC,12
02800			WRH:	SOJGE TAC,WRH
02900				JRST DACRED]
03000		DATAI DB,DACVAL
03100		CONSZ DB,10000
03200		JRST DACDIE
03300		XOR DACVAL,SBMSK
03400	IFN TRACK,<PUSH DATA,DACVAL>
03500		JRST @READFN
03600	FREAD:	ADDI K,770000
03700	IFN TRACK,<SKIPN WALK>
03800		CONO AD,4000(K)	;STOP AD ON NEXT CHANNEL
03900		POPJ P,
04000	READFN:	FREAD
04100	
04200		XLIST
04300		SUBTTL	CONSTANTS				LOU PAUL
04400		LIST
     

00100	NON:	POINT 12,INNER(AC),11
00200		POINT 12,INNER(AC),23
00300		POINT 12,INNER(AC),35
00400		POINT 12,OUTER(AC),11
00500		POINT 12,OUTER(AC),23
00600		POINT 12,OUTER(AC),35
00700	ERR:	0.05
00800		0.05
00900		0.01
01000		0.1
01100		0.1
01200		0.7
01300	TH:	BLOCK 6		;THE NEXT 7 LOCATIONS ARE BLOCK TRANSFERED.
01400	HAND:	3.0
01500	TDD:	BLOCK 6
01600	ERRINT:	0
01700		0
01800		0
01900		0
02000		0
02100		0
02200	DTH:	BLOCK 6
02300	TD:	BLOCK 6
02400	T0P:	BLOCK 6
02500	TDP:	BLOCK 6
02600	ET0:	BLOCK 6
02700	T0:	-150.0
02800		-75.0
02900		14.0
03000		-90.0
03100		 60.0
03200		 0.0000
03300	TFF:	-150.0
03400		-75.0
03500		14.0
03600		-90.0
03700		 60.0
03800		 0.0000
03900	DELTH:	BLOCK 6		;THIS AND THE NEXT ARE BLT ED TOGETHER
04000	DDTH:	0
04100		0
04200		0
04300		0
04400		0
04500		0
04600	NTICKS:	0
04700	DTHS:	BLOCK =30
04800	TORE:	BLOCK 6
04900	T:	BLOCK 6
05000	IFN USER+REPORT+TRACK+DEB,<MOTOR:	BLOCK 6>
05100	FBI:	252502
05200	VZ:	BLOCK 3
05300	IFN TACH,<
05400	VDTH:	BLOCK 3
05500	>
05600	THP:	BLOCK 6
05700	MOTARM:	840.0
05800		900.0
05900		70.0
06000		55.0
06100		55.0
06200		64.0
06300	STQ:	BLOCK 6
06400	SDTQ:	0
06500	ASTP:	0
06600	ASTPP:	0
06700	PC:	BLOCK 6
06800	PCC:	BLOCK 6
06900	DCI:	BLOCK 6
07000	DCII:	BLOCK 6
07100	CI:	0
07200		0
07300		0
07400		0
07500		0
07600		0
07700	CII:	1000.0
07800		1200.0
07900		100.0
08000		100.0
08100		100.0
08200		100.0
08300	NC:	0
08400		0
08500		0
08600		0
08700		0
08800		0
08900	NCC:	1000.0
09000		1200.0
09100		100.0
09200		100.0
09300		100.0
09400		100.0
10200	IFN DEB,<DACERR:	0>
10300	REFTR:	0
10400	ETIME:	=8888
10500	TDF:	0.5
10600	IFN DEB,<
10700	ETIM:	0
10800	ACSAV:	BLOCK 20
10900	>
11000	PDL:	DHO
11100		BLOCK PDLL
11200	LTIME:	0
11300	FUNCT:	0
11400	ROT6:	0
11500	TRAJER:	0
11600	DOING:	0
11700	DATMSD:	0
11800	CBUFS:	0
11900	DATWDS:	0
12000	SNUM:	POINT 12,DACVAL,23
12100	SBMSK:	400040004000
12200	REV:	0
12300		0
12400		-1
12500		0
12600		-1
12700		-1
12800	DMASK:	400000
12900		100000
13000		20000
13100		4000
13200		1000
13300		200
13400	BMASK:	XWD 400000,200000
13500		XWD 200000, 40000
13600		XWD 100000, 10000
13700		XWD  40000,  2000
13800		XWD  20000,   400
13900		XWD  10000,   100
14000	GMASK:	200000
14100		 40000
14200		 10000
14300		  2000
14400		   400
14500		   100
14600	VCHAN:	52B23
14700		53B23
14800		54B23
14900	CHAN:	55B23
15000		56B23
15100		57B23
15200		60B23
15300		61B23
15400	GAIN:	MOVEI I,5
15500	SETGN:	MOVE AC,KV(I)
15600		FMPR AC,AC
15700		FMPR AC,THIRD
15800		MOVEM AC,KE(I)
15900		FMPR AC,KV(I)
16000		FMPR AC,SIXTH
16100		MOVEM AC,KI(I)
16200		SOJGE I,SETGN
16300		MOVE 1,[SIXBIT/[ARM]/]
16400		CALLI 1,400002
16500		MOVE I,RET
16600		MOVEM I,GAIN
16700		JRST GAIN
16800	RET:	POPJ P,
16810	THIRD:	0.3
16820	SIXTH:	0.1
16900		BLOCK 7
17000		0
17100	WAITIN:	XWD 3,0
17200	JUMPBK:	XWD JMPABS,WAITIN
17300	STKPTR:	WAITIN
17400	ERRSTK:	0
17500	BITSAV:	0
17600	COUNT:	0
17700	STWT:	0
17800	HCNT:	0
17900	TP:	0
18000		0
18100		0
18200		0
18300		0
18400		0
18500	TJ:	BLOCK 6
18600	LOOP:	BLOCK 10
18700	TN:	BLOCK 6
18800	SC:	BLOCK 6
18900	A0:	BLOCK 6
19000	A1:	BLOCK 6
19100	A2:	BLOCK 6
19200	A3:	BLOCK 6
19300	A4:	BLOCK 6
19400	BLOCKS:	0
19500	NXTBUF:	0
19600	BUF:	0
19700		BLOCK 1000
19800	BUF2:	0
19900		BLOCK 1000
20000	IFN ADJUST<
20100	ADJI:	-1
20200	ADJE:	0
20300	ADJV:	0
20400	ADJIF:	0
20500	FACTOR:	BLOCK 3
20600	>
20700	IFN REPORT+TRACK,<
20800	DATIWD:	0
20900		0
21000	DATPT:	0
21100	DATO:	BLOCK DBL+100
21200	>
21300	IFN DYNAMICS,<
21400	DDAT:	<SIXBIT/FORD/>+6
21500	FOR:	BLOCK 6
21600		<SIXBIT/BACK/>+6
21700	BAK:	BLOCK 6
21800	>